#include "SteeringEvade.h"

#include <cassert>

#include "ai/steering/SteeringFlee.h"
#include "ai/steering/SteeringOutput.h"
#include "math/Point3.h"
#include "math/Utils.h"
#include "math/Vector3.h"
#include "physic/PhysicConstants.h"

EvadeData::EvadeData(const Point3* srcPos /*= nullptr*/,
                     const Vector3* srcLinearVel /*= nullptr*/,
                     const Point3* targetPos /*= nullptr*/,
                     const Vector3* targetLinearVel /*= nullptr*/,
                     const float maxPredictionTime /*= 0.0f*/)
                     : mSrcPos(srcPos)
                     , mSrcLinearVel(srcLinearVel)
                     , mTargetPos(targetPos)
                     , mTargetLinearVel(targetLinearVel)
                     , mMaxPredictionTime(maxPredictionTime)
{

}

void EvadeData::set(const Point3& srcPos,
         const Vector3& srcLinearVel,
         const Point3& targetPos,
         const Vector3& targetLinearVel,
         const float maxPredictionTime)
{
    mSrcPos = &srcPos;
    mSrcLinearVel = &srcLinearVel;
    mTargetPos = &targetPos;
    mTargetLinearVel = &targetLinearVel;
    mMaxPredictionTime = maxPredictionTime;
}

void evade(EvadeData * const data, 
           SteeringOutput * const outputs,
           const uint32_t numData)
{
    assert(data);
    assert(outputs);
    assert(numData > 0);

    // PARALLEL_FOR
    for (size_t i = 0; i < numData; ++i) {
        const Point3& srcPos = *data[i].mSrcPos;
        const Vector3& srcLinearVel = *data[i].mSrcLinearVel;
        const Point3& targetPos = *data[i].mTargetPos;
        const Vector3& targetLinearVel = *data[i].mTargetLinearVel;
        const float maxPredictionTime = data[i].mMaxPredictionTime;
        SteeringOutput& output = outputs[i];

        // Get the direction and distance to the target
        Vector3 direction;
        direction.set(srcPos, targetPos);
        const float distance = direction.length();

        // Init speed that will be used in flee steering behavior
        // If entity is stopped, we will give it max speed
        float speed = srcLinearVel.length();
        if (areEquals(speed, 0.0f)) {
            speed = MAX_SPEED;
        }

        // Check if speed is too small to give 
        // a reasonable prediction time
        float predictionTime;
        if (speed < distance / maxPredictionTime || 
            areEquals(speed, distance / maxPredictionTime))
        {
            predictionTime = maxPredictionTime;
        } else {
            predictionTime = distance / speed;
        }

        // Update target position to predicted position            
        Vector3 scaledTargetLinearVel;
        scaledTargetLinearVel = targetLinearVel * predictionTime;
        Point3 predictedTargetPos(targetPos);
        predictedTargetPos += scaledTargetLinearVel;

        // Init flee data used in steering behavior seek
        FleeData fleeData(&srcPos, &predictedTargetPos, speed);

        // Execute flee steering behavior
        flee(&fleeData, &output, 1);
    }
}